
#include "vi5300.h"
#include "vi5300Firmware.h"
#include "i2c.h"
#include <math.h>
#include "hc32_ll_utility.h"
#include <stdlib.h>
#include <stdio.h>

#define VI5300_DEVICE_ADDR 0x6C
#define VI5300_REG_MCU_CFG 0x00
#define VI5300_RET_INT_STATUS 0x03
#define VI5300_REG_SYS_CFG 0x01
#define VI5300_REG_PW_CTRL 0x07
#define VI5300_REG_CMD 0x0a
#define VI5300_REG_SIZE 0x0b
#define VI5300_REG_SCRATCH_PAD_BASE 0x0c

#define VI5300_WRITEFW_CMD 0x03
#define VI5300_USER_CFG_CMD 0x09
#define VI5300_START_RANG_CMD 0x0E

#define VI5300_CFG_SUBCMD 0x01
#define VI5300_OTPW_SUBCMD 0x02
#define VI5300_OTPR_SUBCMD 0x03

#define PILEUP_A (9231000)
#define PILEUP_B (4896)
#define PILEUP_C (1922)
#define PILEUP_D (10)

#define XSHUT_Pin1 0      // GPIO_PIN_0
#define XSHUT_Pin2 0      // GPIO_PIN_1
#define XSHUT_Pin3 0      // GPIO_PIN_4
#define XSHUT_GPIO_Port 0 // GPIOA

uint8_t gSalve;
uint8_t chip_reg = 0xff;

/*****添加的程序*****/
uint8_t Calibration_data[10] ={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

typedef struct 
{
		uint8_t Cali_Flag;
    uint8_t xtalk_cal;
		int16_t offset_cal;
		int16_t RefTof;
}VI5300_CALIBRATION_DATA;
VI5300_CALIBRATION_DATA  cali_data;
/*******************/

 
typedef struct 
{
    int8_t xtalk_cal;
    uint16_t xtalk_peak;

} VI5300_XTALK_Calib_Data;

typedef struct OFFSET_CALIB
{
    int16_t offset_cal;
	 int16_t RefTof;

} VI5300_OFFSET_Calib_Data;

typedef enum
{
    VI5300_OK = 0x00,
    VI5300_RANGING = 0x01,
    VI5300_BUSY = 0x02,
    VI5300_BUS_BUSY = 0x03,
    VI5300_SLEEP = 0x04,
    VI5300_BOOTING = 0x05,
    VI5300_ERROR = 0x06
} VI5300_Status;

typedef enum
{
    VI5300_DATA_VALID = 0x00,
    VI5300_RAW_DATA = 0x01,
    VI5300_DATA_ERROR = 0x02
} VI5300_RangingDataStatus;
typedef enum NEXTACTION
{
    VI5300_GO_ON = 0x00,
    VI5300_STOP = 0x01
}VI5300_NextAction;

VI5300_XTALK_Calib_Data xtalk_cal; 

VI5300_OFFSET_Calib_Data offset_data;
// 判断芯片是否存在
uint8_t existenceFlag = 0;

VI5300_Dist_TypeDef distance;

void ReadCalibrationDataFromFile(VI5300_CALIBRATION_DATA *cali_data) 
{
	union byte_int16
	{
		uint8_t byte[2];
		int16_t int16_data;
	}	byte_to_int16;
	
	//readFlashPage(FLASH_SECTOR63_ADRR, (uint32_t *)Calibration_data,6);
	
	cali_data->Cali_Flag = Calibration_data[0];
	cali_data->xtalk_cal = Calibration_data[1];
	byte_to_int16.byte[0] = Calibration_data[2];
	byte_to_int16.byte[1] = Calibration_data[3];
	cali_data->offset_cal = byte_to_int16.int16_data;
	byte_to_int16.byte[0] = Calibration_data[4];
	byte_to_int16.byte[1] = Calibration_data[5];
	cali_data->RefTof = byte_to_int16.int16_data;
}	
	


void VI5300_Set_Digital_Clock_Dutycycle(void)
{
    uint8_t t_reg;
    t_reg = 0x0F;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x0E;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    if (chip_reg == 0x00)
    {
        DDL_DelayMS(1);
        t_reg = 0x24;
        i2c3Write(VI5300_DEVICE_ADDR, 0xE9, &t_reg, 1);
        t_reg = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0xEE, &t_reg, 1);
        t_reg = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0xF5, &t_reg, 1);
    }
    DDL_DelayMS(4); //默认4ms ，延时不准可以增加
}

void VI5300_WriteFirmwarePostConfig(void)
{
    uint8_t t_reg;
    t_reg = 0x0c;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_SYS_CFG, &t_reg, 1);
    t_reg = 0x06;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_MCU_CFG, &t_reg, 1);
    t_reg = 0xA0;
    i2c3Write(VI5300_DEVICE_ADDR, 0x3B, &t_reg, 1);
    t_reg = 0x80;
    i2c3Write(VI5300_DEVICE_ADDR, 0x3B, &t_reg, 1);

    if (chip_reg == 0x00)
    {
        t_reg = 0x24;
        i2c3Write(VI5300_DEVICE_ADDR, 0xE9, &t_reg, 1);
        t_reg = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0xEE, &t_reg, 1);
        t_reg = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0xF5, &t_reg, 1);
    }

    t_reg = 0x07;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_MCU_CFG, &t_reg, 1);
    t_reg = 0x02;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x00;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
}

void VI5300_WriteFirmware32Byte(uint8_t len, uint8_t *data)
{
    uint8_t cmd_size[2];

    if (len > 32)
        return;

    cmd_size[0] = VI5300_WRITEFW_CMD;
    cmd_size[1] = len;

    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, cmd_size, 2);
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, data, len);
}

void VI5300_WriteFirmwarePreConfig(void)
{
    uint8_t t_reg = 0x01;
    VI5300_Set_Digital_Clock_Dutycycle(); // set rco config add 20210129
    t_reg = 0x08;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x0a;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_PW_CTRL, &t_reg, 1);
    t_reg = 0x06;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_MCU_CFG, &t_reg, 1);
    i2c3Read(VI5300_DEVICE_ADDR, VI5300_REG_SYS_CFG, &t_reg, 1);
    t_reg |= 0x01;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_SYS_CFG, &t_reg, 1);

    if (chip_reg == 0x00)
    {
        t_reg = 0x30;
        i2c3Write(VI5300_DEVICE_ADDR, 0x38, &t_reg, 1);
        i2c3Write(VI5300_DEVICE_ADDR, 0x3A, &t_reg, 1);
    }

    t_reg = 0x01;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, &t_reg, 1);
    t_reg = 0x02;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_SIZE, &t_reg, 1);
    t_reg = 0x00;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, &t_reg, 1);
    t_reg = 0x00;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE + 0x01, &t_reg, 1);
}

void VI5300_DownloadFirmware(uint8_t *buf, uint16_t size)
{

    uint8_t page = 0;

    VI5300_WriteFirmwarePreConfig();
    while (size >= 32)
    {
        VI5300_WriteFirmware32Byte(32, buf + page * 32);
        size -= 32;
        page++;
    }

    if (size > 0)
    {
        VI5300_WriteFirmware32Byte(size, buf + page * 32);
    }

    VI5300_WriteFirmwarePostConfig();
}

void VI5300_Interrupt_Enable(void)
{
    uint8_t loop = 0;
    uint8_t enable = 0;

    do
    {
        i2c3Read(VI5300_DEVICE_ADDR, 0x04, &enable, 1);
        enable |= 0x01;
        i2c3Write(VI5300_DEVICE_ADDR, 0x04, &enable, 1);
        i2c3Read(VI5300_DEVICE_ADDR, 0x04, &enable, 1);
        loop++;
    } while ((loop < 5) && (!(enable & 0x01)));

    if (loop >= 5)
    {
        // 中断出错
        existenceFlag = 0;
    }
}

void VI5300_Integral_Counts_Write(uint32_t inte_counts)
{
    uint8_t buf[4] = {0};
    //小端模式，从小到大保存
    buf[0] = (inte_counts)&0xFF;
    buf[1] = (inte_counts >> 8) & 0xFF;
    buf[2] = (inte_counts >> 16) & 0xFF;

    VI5300_Set_Digital_Clock_Dutycycle();
    uint8_t t_reg;
    t_reg = 0x01;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x03;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x01;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    t_reg = buf[0];
    i2c3Write(VI5300_DEVICE_ADDR, 0x0F, &t_reg, 1);
    t_reg = buf[1];
    i2c3Write(VI5300_DEVICE_ADDR, 0x10, &t_reg, 1);
    t_reg = buf[2];
    i2c3Write(VI5300_DEVICE_ADDR, 0x11, &t_reg, 1);
    t_reg = 0x09;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
}

void VI5300_Delay_Count_Write(uint16_t delay_count)
{
    uint8_t buf[2] = {0};

    //大端模式
    buf[0] = (delay_count >> 8) & 0xFF;
    buf[1] = (delay_count)&0xFF;

    VI5300_Set_Digital_Clock_Dutycycle();

    uint8_t t_reg;
    t_reg = 0x01;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
    t_reg = 0x02;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
    t_reg = 0x04;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
    //大端模式
    t_reg = buf[0];
    i2c3Write(VI5300_DEVICE_ADDR, 0x0F, &t_reg, 1);
    t_reg = buf[1];
    i2c3Write(VI5300_DEVICE_ADDR, 0x10, &t_reg, 1);
    t_reg = 0x09;
    i2c3Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
}

void VI5300_Set_Integralcounts_Frame(uint8_t fps, uint32_t intecoutns)
{
    uint32_t inte_time;
    uint32_t fps_time;
    uint32_t delay_time;
    uint16_t delay_counts;

    inte_time = intecoutns * 1463 / 10;
    fps_time = 1000000000 / fps;
    delay_time = fps_time - inte_time - 1600000;
    delay_counts = (uint16_t)(delay_time / 40900);

    VI5300_Integral_Counts_Write(intecoutns);
    VI5300_Delay_Count_Write(delay_counts);
}

void VI5300_Wait_For_CPU_Ready(void)
{
    uint8_t stat;
    int retry = 0;

    do
    {
        DDL_DelayMS(1); // delay 1ms
        i2c3Read(VI5300_DEVICE_ADDR, 0x02, &stat, 1);
    } while ((retry++ < 20) && (stat & 0x01));
    if (retry >= 20)
    {
        existenceFlag = 0;
    }
}

VI5300_Status VI5300_Get_Interrupt_State(uint8_t *status)
{
    return	(VI5300_Status)i2c3Read(VI5300_DEVICE_ADDR, VI5300_RET_INT_STATUS, status, 1);
}
VI5300_Status WriteCommand(uint8_t cmd)
{
	return (VI5300_Status)i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, &cmd, 1);
}

void VI5300_Start_Continuous_Measure(void)
{
    uint8_t stat = 0;

    i2c3Read(VI5300_DEVICE_ADDR, VI5300_RET_INT_STATUS, &stat, 1);

    VI5300_Wait_For_CPU_Ready();

    VI5300_Set_Digital_Clock_Dutycycle();

    uint8_t t_reg = 0x0F;
    i2c3Write(VI5300_DEVICE_ADDR, VI5300_REG_CMD, &t_reg, 1);
}


#ifdef VI5300_TEMP_CALIBRATION //vi5300校准***********
VI5300_Status VI5300_Temp_Enable(uint8_t enable)
{
	uint8_t t_reg;

	t_reg = 0x01;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
	t_reg = 0x01;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
	t_reg = 0x0E;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
	t_reg = enable;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0F, &t_reg, 1);
	t_reg = 0x09;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
	DDL_DelayMS(5);
	
//	WriteOneReg(0x0C, 0x01);
//	WriteOneReg(0x0D, 0x01);
//	WriteOneReg(0x0E, 0x0E);
//	WriteOneReg(0x0F, enable);
//	WriteOneReg(0x0A, 0x09);


}
VI5300_Status VI5300_Check_Temp_Enable(uint8_t *enable)
{
	uint8_t t_reg;
	uint8_t tmp=0;
	t_reg = 0x00;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
	t_reg = 0x01;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
	t_reg = 0x0E;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
	t_reg = 0x09;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1);
	DDL_DelayMS(5);
	tmp = 0x09;
	i2c3Read(VI5300_DEVICE_ADDR, 0x0c, &tmp, 1);
	*enable = tmp;
	
}

VI5300_Status VI5300_Config_RefTof_Parameter(int16_t refTof)
{
	uint8_t tmp_tof;
	uint8_t t_reg;
	
    VI5300_Wait_For_CPU_Ready();
		VI5300_Set_Digital_Clock_Dutycycle();
	
	t_reg = 0x01;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
	t_reg = 0x02;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
	t_reg = 0x17;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
		tmp_tof=refTof>>8;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0F, &tmp_tof, 1);
		tmp_tof=refTof&0xFF;
	i2c3Write(VI5300_DEVICE_ADDR, 0x10, &tmp_tof, 1);
	t_reg = 0x09;
	i2c3Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1); 
}

VI5300_Status VI5300_Check_RefTof_Parameter(int16_t *refTof)
{
	uint8_t t_reg;
	uint8_t tmp=0;
		
    VI5300_Wait_For_CPU_Ready();
		VI5300_Set_Digital_Clock_Dutycycle();
	
	t_reg = 0x00; 
	i2c3Write(VI5300_DEVICE_ADDR, 0x0C, &t_reg, 1);
	t_reg = 0x02; 
	i2c3Write(VI5300_DEVICE_ADDR, 0x0D, &t_reg, 1);
	t_reg = 0x17; 
	i2c3Write(VI5300_DEVICE_ADDR, 0x0E, &t_reg, 1);
	t_reg = 0x09; 
	i2c3Write(VI5300_DEVICE_ADDR, 0x0A, &t_reg, 1); 
	
	DDL_DelayMS(5);
    i2c3Read(VI5300_DEVICE_ADDR, 0x0c, &tmp, 1);
	*refTof=tmp<<8;
    i2c3Read(VI5300_DEVICE_ADDR, 0xd, &tmp, 1);
    *refTof |= tmp;
}

void VI5300_Start_RefTof_Calibration(VI5300_OFFSET_Calib_Data* OFFSETCalibData)
{
	uint8_t retry=0;
	uint8_t stat=0xff,buf[32];	
	int16_t milimeter;
	uint32_t peak2,peak_tmp;
	uint32_t integral_times;
	int32_t bias;
	int16_t i_reftof=0;
	int16_t total = 0;
	int cnt = 0;
	#ifdef VI5300_OFFSET_CALIBRATION
	int16_t ioffset = 0;
	#endif

	do
	{
		VI5300_Temp_Enable(0);
		DDL_DelayMS(2);
		VI5300_Check_Temp_Enable(&stat);
	}while((stat!=0)&&(retry++<5));
	
	DDL_DelayMS(5);
	VI5300_Start_Continuous_Measure(); 
	while(1)
	{
		DDL_DelayMS(30);
		VI5300_Get_Interrupt_State(&stat);
		if((stat & 0x01) == 0x01)
		{
				i2c3Read(VI5300_DEVICE_ADDR, VI5300_REG_SCRATCH_PAD_BASE, buf, 32);
				i_reftof +=(short)((((short)buf[21])<<8) |(( (short)buf[20])));
				milimeter = (int16_t)((((int16_t)buf[13])<<8) |(( (int16_t)buf[12])));

				peak2 = (uint32_t)((((uint32_t)buf[11])<<24) |( ((uint32_t)buf[10])<<16)|(((uint32_t)buf[9])<<8)|( (uint32_t)buf[8]));

				integral_times = (uint32_t)((((uint32_t)buf[25])<<24) |( ((uint32_t)buf[24])<<16)|( ((uint32_t)buf[23])<<8)|( (uint32_t)buf[22]));
				integral_times = integral_times & 0x00ffffff;
				if(peak2 > 65536) {
						peak_tmp = (peak2 * 256) / integral_times * 256;
				} else {
						peak_tmp = peak2 * 65536 / integral_times;
				}
				
				peak_tmp = peak_tmp >> 12;
				bias = (int32_t)(PILEUP_A / (PILEUP_B - peak_tmp * PILEUP_D) - PILEUP_C) / PILEUP_D;
				if(bias < 0)
				{
					bias = 0;
				}
				milimeter = milimeter + (int16_t)bias;
				total += milimeter;
				cnt++;
		}
		if(cnt>=20)
		break;
	}
	retry=0;
	VI5300_Stop_Continuous_Measure();
	OFFSETCalibData->RefTof =i_reftof/20;
	#ifdef VI5300_OFFSET_CALIBRATION
	ioffset = total / 20;
	OFFSETCalibData->offset_cal = ioffset - OFFSET_POSITION;   //此处100是指在10cm处做offset标定，在多远距离做标定减多少，单位mm
	#endif
	stat=0;
	do
	{
			VI5300_Temp_Enable(1);
			DDL_DelayMS(2);
			VI5300_Check_Temp_Enable(&stat);
	}while((stat!=1)&&(retry++<5));
}
void VI5300_Config_RefTof(int16_t reftof)
{
		int16_t R_reftof;
		uint8_t retry=0;
		
		do
		{
				VI5300_Config_RefTof_Parameter(reftof);
				DDL_DelayMS(2);
				VI5300_Check_RefTof_Parameter(&R_reftof);
		}while((R_reftof!=reftof)&&retry++<5);
		DDL_DelayMS(2);
}
#endif //******以上，vi5300校准***********
uint8_t VI5300_Stop_Continuous_Measure(void)
{
    VI5300_Status ret = VI5300_OK;
    
    ret = WriteCommand(0x1F);
		DDL_DelayMS(10);
    return ret;
}
//
uint8_t vi5300Init(void)
{
    uint8_t stat = 0;
	uint8_t retry = 0;
    uint8_t t_reg = 0xFF,t_reg2,chip_reg3a;//t_reg1 = 0xF0,

    i2c3Read(VI5300_DEVICE_ADDR, 0x06, &t_reg2, 1);
    if (t_reg2 == 0xD8)
    {
		for(retry = 0; retry < 5; retry++)
		{
				i2c3Read(VI5300_DEVICE_ADDR, 0x38, &t_reg, 1);
				if(t_reg == 0x30)
				{
						i2c3Read(VI5300_DEVICE_ADDR, 0x3A, &chip_reg3a, 1); 
						if(chip_reg3a == 0x30)
						{
							chip_reg=0x30;    //应该使用这个版本的3.1版本的tof芯片                                                                          
							break;
						} 
				} 
 
		} 
		if(chip_reg==0x00)
		{
			return 0;
		}
		
        DDL_DelayMS(500);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x00, &t_reg2, 1);
        t_reg2 = 0x0c;
        i2c3Write(VI5300_DEVICE_ADDR, 0x01, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x01;
        i2c3Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x21;
        i2c3Write(VI5300_DEVICE_ADDR, 0x04, &t_reg2, 1);
        t_reg2 = 0x0E;
        i2c3Write(VI5300_DEVICE_ADDR, 0x05, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x08, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c3Write(VI5300_DEVICE_ADDR, 0x37, &t_reg2, 1);
        t_reg2 = 0x30;
        i2c3Write(VI5300_DEVICE_ADDR, 0x38, &t_reg2, 1);
        t_reg2 = 0x30;
        i2c3Write(VI5300_DEVICE_ADDR, 0x3A, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x39, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c3Write(VI5300_DEVICE_ADDR, 0x3B, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c3Write(VI5300_DEVICE_ADDR, 0x3C, &t_reg2, 1);
        t_reg2 = 0x80;
        i2c3Write(VI5300_DEVICE_ADDR, 0x3D, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x3E, &t_reg2, 1);
        t_reg2 = 0x00;
        i2c3Write(VI5300_DEVICE_ADDR, 0x3F, &t_reg2, 1);
        t_reg2 = 0x0E;
        i2c3Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);
        t_reg2 = 0x0F;
        i2c3Write(VI5300_DEVICE_ADDR, 0x07, &t_reg2, 1);

        retry = 0;
        do
        {
            DDL_DelayMS(1);                              // delay 1ms
            i2c3Read(VI5300_DEVICE_ADDR, 0x02, &stat, 1); // stat = 0x10
        } while ((retry++ < 20) && (stat & 0x01));

//        i2c3Read(VI5300_DEVICE_ADDR, 0x08, &stat, 1);
//        Ddl_Delay1ms(40);
//        do
//        {
//            t_reg2 = 0x00;
//            i2c3Write(VI5300_DEVICE_ADDR, 0x08, &t_reg2, 1);
//            Ddl_Delay1ms(1);
//            i2c3Read(VI5300_DEVICE_ADDR, 0x08, &stat, 1);
//        } while (stat != 0 && retry++ < 5);
        // 芯片固件开始启动，大约持续 40 ms，
        // 可以通过确认 0x08 寄存器是否为 0x66 来判断是否启动成功。
        // 固件启动后，芯片会自动进入软件休眠模式。
        retry = 0;	
        do
        {
            VI5300_DownloadFirmware(g_pVi5300Firmware, getFirmwareLen());
            DDL_DelayMS(10); // delay 10ms
            i2c3Read(VI5300_DEVICE_ADDR, 0x08, &stat, 1);
        } while ((stat != 0x55) && (stat != 0x66) && (retry++ < 5));

		
		if (retry>= 5)
        {
			existenceFlag=0;
			return existenceFlag;
		}
			
		existenceFlag = 1;
		VI5300_Interrupt_Enable(); 
		VI5300_Set_Integralcounts_Frame(30, 131072);
		/**添加的程序****/
		ReadCalibrationDataFromFile(&cali_data);
		if(cali_data.Cali_Flag == 0xFF)
		{
					cali_data.Cali_Flag =0;
		}
		
		#ifdef VI5300_TEMP_CALIBRATION
		if((cali_data.Cali_Flag&0xF0)!=0x10)
		{
		//标定在第一次开机执行，或者工厂模式执行，后面不能执行 
			VI5300_Start_RefTof_Calibration(&offset_data);
			cali_data.Cali_Flag |= 0x10;//RefTof&offset calibration flag
			cali_data.RefTof =offset_data.RefTof;
			cali_data.offset_cal =offset_data.offset_cal;
		}
		else{
			offset_data.RefTof = cali_data.RefTof;
			offset_data.offset_cal = cali_data.offset_cal;
		}
		
		VI5300_Config_RefTof(offset_data.RefTof);
		#endif
		/*****/
		// 开始测量
		VI5300_Start_Continuous_Measure();
    }
    return existenceFlag;
}

void VI5300_GetRawRangingData(VI5300_Dist_TypeDef *result)
{
    uint8_t buf[32],i;
    int16_t milimeter;
    uint16_t noise,noise_level;
    uint32_t peak1;
    uint32_t peak2,peak1_r,noise_r;
    uint32_t integral_times;
    uint32_t peak_tmp;
	uint32_t lower;
	uint32_t upper;
    int32_t bias;
    uint32_t confidence;
	uint32_t s_noise[10] = {19, 83, 163, 691, 1243, 2539, 5091, 10395, 20427, 33235};
	uint32_t s_lower[10] = {16, 25, 45, 145, 258, 501, 1004, 1980, 3804, 6063};
	uint32_t s_upper[10] = {26, 35, 61, 173, 303, 574, 1148, 2220, 4230, 6705};
		
	i2c3Read(VI5300_DEVICE_ADDR,VI5300_REG_SCRATCH_PAD_BASE, buf, 32);

	/***********add the function of pileup and confidence 20210105  小端模式*********/
	milimeter = (short)((((short)buf[13])<<8) |(( (short)buf[12])));
	peak1 = (uint32_t)((((uint32_t)buf[31])<<24) |((uint32_t)buf[30]<<16)|((uint32_t)buf[29]<<8)|((uint32_t)buf[28]));
	noise_level = (uint16_t)((((uint16_t)buf[27])<<8) |(((uint16_t) buf[26])));
	peak2 = (uint32_t)((((uint32_t)buf[11])<<24) |( ((uint32_t)buf[10])<<16)|(((uint32_t)buf[9])<<8)|( (uint32_t)buf[8]));

	integral_times = (uint32_t)((((uint32_t)buf[25])<<24) |( ((uint32_t)buf[24])<<16)|( ((uint32_t)buf[23])<<8)|( (uint32_t)buf[22]));
    integral_times = integral_times & 0x00ffffff;
		
    peak_tmp = peak2 * 16 /integral_times;
    bias = (int32_t)(PILEUP_A / (PILEUP_B - peak_tmp * PILEUP_D) - PILEUP_C) / PILEUP_D;
    if(bias < 0)
		{
				bias = 0;
		}
		milimeter = milimeter + (int16_t)bias;
		
		#ifdef VI5300_OFFSET_CALIBRATION
		milimeter = milimeter - offset_data.offset_cal;//减去offset
		#endif
		////update by 2022/03/07  
		if(peak1 > 8000000)
			peak1_r = peak1 * 256 / integral_times * 4;
		else if(peak1 > 4000000)
			peak1_r = peak1 * 512 / integral_times * 2;
		else
			peak1_r = peak1 * 1024 / integral_times;

		noise_r = noise_level * 65536 /integral_times* 2;

		for(i = 0; i < 9; i++)
		{
			if(noise_r < s_noise[i + 1])
			{
				lower = (s_lower[i + 1] - s_lower[i]) * abs(noise_r - s_noise[i]) / (s_noise[i + 1] - s_noise[i]) + s_lower[i];
				upper = (s_upper[i + 1] - s_upper[i]) * abs(noise_r - s_noise[i]) / (s_noise[i + 1] - s_noise[i]) + s_upper[i];
				break;
			} else if(noise_r >= s_noise[9]){
				lower = (s_lower[9] - s_lower[8]) * abs(noise_r - s_noise[8]) / (s_noise[9] - s_noise[8]) + s_lower[8];
				upper = (s_upper[9] - s_upper[8]) * abs(noise_r - s_noise[8]) / (s_noise[9] - s_noise[8]) + s_upper[8];
				break;
			}
		}
		if(peak1_r < lower) {
			confidence = 0;
		} else if(peak1_r > upper) {
			confidence = 100;
		} else {
			confidence = 100 * (peak1_r - lower) / (upper - lower);
		}

//		if(confidence >= 90)
//		{
//				result->status = STATUS_TOF_CONFIDENT;
//		}
//		else if(confidence >= 30)
//		{
//				result->status = STATUS_TOF_SEMI_CONFIDENT;
//		}
//		else
//		{
//				result->status = STATUS_TOF_NOT_CONFIDENT;
//		}
//		if(milimeter < 50 && peak1 < 800000)
//		{
//			//confidence =0;
//				result->status = STATUS_TOF_NOT_CONFIDENT;
//		}
		result->millimeter = milimeter;
		result->peak       = peak1;
		result->noise      = noise;
		result->confidence = confidence;
}


uint8_t VI5300_Get_Measure_Data(uint16_t *dist)
{
    uint8_t int_status = 0;
    i2c3Read(VI5300_DEVICE_ADDR, VI5300_RET_INT_STATUS, &int_status, 1);
    if ((int_status & 0x01) == 0x00)
    {
        return 0;	
    }
    VI5300_GetRawRangingData(&distance); 
	//printf("mill=%5d,peak=%5d,nois=%5d,conf=%5d\r\n",distance.millimeter,distance.peak,distance.noise,distance.confidence);
 	if(distance.confidence>=30 && distance.millimeter > 0)
	{
		*dist = distance.millimeter /10;	    // dist单位为cm
	}
 	else
	{
 		return 0;
	}
    return 1;
}


